#include "Process.h"
using namespace std;
using namespace cv;

//两个图片相乘求相关性
uint64 CorrelationAVX2(cv::Mat& imgA, cv::Mat& imgB)
{
	const int A = 32;//AVX2
	assert(imgA.cols >= A);
	size_t bodyWidth = AlignLo(imgA.cols, A);
	bool EndType = bodyWidth != imgA.cols;
	size_t Signm = 0;

	__m256i tailMask;
	if (EndType)
	{
		int position = A - imgA.cols + bodyWidth;
		static uchar P[32] = { 0 };
		for (size_t i = 0; i < position; ++i)
			P[i] = 0;
		for (size_t i = position; i < 32; ++i)
			P[i] = 0xFF;
		tailMask = _mm256_loadu_si256((__m256i*)P);
		Signm = imgA.cols - A;
	}

	uchar* PtrA;
	uchar* PtrB;
	__m256i fullSum = _mm256_setzero_si256();
	__m256i rowSum = _mm256_setzero_si256();
	for (size_t i = 0; i < imgA.rows; i++)
	{
		rowSum = _mm256_setzero_si256();
		PtrA = imgA.ptr<uchar>(i);
		PtrB = imgB.ptr<uchar>(i);
		for (size_t j = 0; j < bodyWidth; j+=A)
		{
			const __m256i a_ = _mm256_loadu_si256((__m256i*)(PtrA + j));
			const __m256i b_ = _mm256_loadu_si256((__m256i*)(PtrB + j));
			rowSum = _mm256_add_epi32(rowSum, Correlation(a_, b_));
		}
		if (EndType)
		{
			const __m256i a_ = _mm256_and_si256(tailMask, _mm256_loadu_si256((__m256i*)(PtrA + Signm)));
			const __m256i b_ = _mm256_and_si256(tailMask, _mm256_loadu_si256((__m256i*)(PtrB + Signm)));
			rowSum = _mm256_add_epi32(rowSum, Correlation(a_, b_));
		}
		fullSum = _mm256_add_epi64(fullSum, HorizontalSum32(rowSum));
	}
	uint64 sum = 0;
	for (size_t i = 0; i < 4; i++)
	{
		sum += fullSum.m256i_i64[i];
	}
	return sum;
}

uint64 CorrelationSSE(cv::Mat& imgA, cv::Mat& imgB)
{
	const int A = 16;//SSE
	assert(imgA.cols >= A);
	size_t bodyWidth = AlignLo(imgA.cols, A);
	bool EndType = bodyWidth != imgA.cols;
	size_t Signm = 0;

	__m128i tailMask;
	if (EndType)
	{
		int position = A - imgA.cols + bodyWidth;
		static uchar P[A] = { 0 };
		for (size_t i = 0; i < position; ++i)
			P[i] = 0;
		for (size_t i = position; i < A; ++i)
			P[i] = 0xFF;
		tailMask = _mm_loadu_si128((__m128i*)P);
		Signm = imgA.cols - A;
	}

	uchar* PtrA;
	uchar* PtrB;
	__m128i fullSum = _mm_setzero_si128();
	__m128i rowSum = _mm_setzero_si128();
	for (size_t i = 0; i < imgA.rows; ++i)
	{
		PtrA = imgA.ptr<uchar>(i);
		PtrB = imgB.ptr<uchar>(i);
		rowSum = _mm_setzero_si128();
		for (size_t j = 0; j < bodyWidth; j += A)
		{
			const __m128i a_ = _mm_loadu_si128((__m128i*)(PtrA + j));
			const __m128i b_ = _mm_loadu_si128((__m128i*)(PtrB + j));
			const __m128i lo = _mm_madd_epi16(_mm_unpacklo_epi8(a_, _mm_setzero_si128()), _mm_unpacklo_epi8(b_, _mm_setzero_si128()));
			const __m128i hi = _mm_madd_epi16(_mm_unpackhi_epi8(a_, _mm_setzero_si128()), _mm_unpackhi_epi8(b_, _mm_setzero_si128()));
			rowSum = _mm_add_epi32(rowSum, _mm_add_epi32(lo, hi));
		}
		if (EndType)
		{
			//const __m128i a_ = _mm_loadu_si128((__m128i*)(PtrA + Signm));
			//const __m128i b_ = _mm_loadu_si128((__m128i*)(PtrB + Signm));
			const __m128i a_ = _mm_and_si128(tailMask, _mm_loadu_si128((__m128i*)(PtrA + Signm)));
			const __m128i b_ = _mm_and_si128(tailMask, _mm_loadu_si128((__m128i*)(PtrB + Signm)));
			const __m128i lo = _mm_madd_epi16(_mm_unpacklo_epi8(a_, _mm_setzero_si128()), _mm_unpacklo_epi8(b_, _mm_setzero_si128()));
			const __m128i hi = _mm_madd_epi16(_mm_unpackhi_epi8(a_, _mm_setzero_si128()), _mm_unpackhi_epi8(b_, _mm_setzero_si128()));
			rowSum = _mm_add_epi32(rowSum, _mm_add_epi32(lo, hi));
		}
		fullSum = _mm_add_epi64(fullSum, HorizontalSum32SSE(rowSum));
	}
	uint64 sum = 0;
	for (size_t i = 0; i < 2; i++)
	{
		sum += fullSum.m128i_i64[i];
	}
	return sum;
}


